use utils::VolatileHandle;

#[derive(Debug)]
#[cfg_attr(feature = "defmt", derive(defmt::Format))]
pub enum Dir {
    Enter,
    Exit,
}

pub trait StepMotorDrv {
    type Error;

    const TIMEOUT_ERR: Self::Error;
    const ALM_ERR: Self::Error;

    fn set_dir(dir: Dir);
    fn set_pwm_freq(freq: u32);
    fn enable_pwm(freq: u32);
    fn enable_pwm_not_irq(freq: u32);
    fn disable_pwm();
    fn is_origin() -> bool;
    fn is_alm() -> bool;
    fn set_puls_pin();
    fn reset_puls_pin();

    fn delay(tick: u32);

    fn notify_run_finish();
    fn wait_run_finish();

    const CHECK_TICK: u32 = 20; // 循环检测间隔
    const POSITION_0_DELAY: u32 = 50; // 到达位置0以后 延迟
    const SLOW_DELAY_TICK: u32 = 2; // 缓慢移动 延迟
    const SLOW_DELAY: u32 = 100;
    const RESET_PULS_COUNT: u32 = 2000; // 复位时 最大的脉冲数量
    const EXIT_PULS_COUNT: u32 = 1000;
    const RESET_TIMEOUT: u32 = 10 * 1000; // 复位超时时间
    const RESET_FREQ: u32 = 800; // 复位时 PWM 频率

    const BASE_FREQ: u32 = 100;
    const ACC: u32 = 2;
    const VAR_RANGE: u32 = 200;
}

pub struct StepMotor<T: StepMotorDrv> {
    _drv: T,
    total: u32,
    current: u32,
    s0: u32,
    s1: u32,
    init_flag: bool,
    position: i32,
}

impl<T: StepMotorDrv> StepMotor<T> {

    pub const fn const_new(drv: T) -> Self {
        Self {
            _drv: drv,
            total: 0,
            current: 0,
            s0: 0,
            s1: 0,
            init_flag: false,
            position: 0,
        }
    }

    pub fn on_puls(&mut self) {
        let current = self.current.volatile_read() + 1;
        self.current.volatile_write(current);

        let total = self.total.volatile_read();
        
        if current >= total {
            T::disable_pwm();
            T::notify_run_finish();
            return;
        }

        if current <= self.s0.volatile_read() {
            T::set_pwm_freq(T::BASE_FREQ + T::ACC * current);
            return;
        }

        if current <= self.s1.volatile_read() {
            return;
        }

        T::set_pwm_freq(T::BASE_FREQ + T::ACC * (total - current));
    }

    // 圆形
    pub fn run_position_with_circle(&mut self, position: i32, circle_length: i32) -> Result<(), T::Error> {
        self.run_position_circle_with_async(position, circle_length)?;
        self.wait_async_finish()?;
        Ok(())
    }

    pub fn run_position(&mut self, position: i32) -> Result<(), T::Error> {
        self.run_position_with_async(position)?;
        self.wait_async_finish()?;
        Ok(())
    }

    // 不改变方向
    pub fn run_position_with_direct(&mut self, position: i32, circle_length: i32) -> Result<(), T::Error> {
        self.run_position_direct_with_async(position, circle_length)?;
        self.wait_async_finish()?;
        Ok(())
    }

    pub fn wait_async_finish(&mut self) -> Result<(), T::Error> {
        T::wait_run_finish();
        if T::is_alm() {
            self.init_flag = false;
            T::notify_run_finish();
            return Err(T::ALM_ERR);
        }
        if self.position == 0 {
            T::delay(T::POSITION_0_DELAY);
            self.reset_slow().map_err(|e| {
                T::notify_run_finish();
                e
            })?;
        }
        T::notify_run_finish();
        Ok(())
    }

    pub fn run_position_direct_with_async(&mut self, position: i32, circle_length: i32) -> Result<(), T::Error> {
        T::wait_run_finish();

        if !self.init_flag {
            self.reset().map_err(|e| {
                T::notify_run_finish();
                e
            })?;
        }
 
        T::set_dir(Dir::Enter);
        T::delay(T::CHECK_TICK);

        if position < self.position {
            let n = position + circle_length - self.position;
            self.run_total(n as u32);
        } else {
            let n = position - self.position;
            self.run_total(n as u32);
        }
        self.position = position;
        Ok(())
    }

    pub fn run_position_circle_with_async(&mut self, position: i32, circle_length: i32) -> Result<(), T::Error> {
        T::wait_run_finish();

        if !self.init_flag {
            self.reset().map_err(|e| {
                T::notify_run_finish();
                e
            })?;
        }

        if position > self.position {
            let p0 = (position - self.position).abs();
            let p1 = (circle_length - position + self.position).abs();

            if p0 <= p1 {
                T::set_dir(Dir::Exit);
                T::delay(T::CHECK_TICK);
                self.run_total(p0 as u32);
            } else {
                T::set_dir(Dir::Enter);
                T::delay(T::CHECK_TICK);
                self.run_total(p1 as u32);
            }
        } else {
            let p0 = (position - self.position).abs();
            let p3 = (circle_length - self.position + position).abs();

            if p0 <= p3 {
                T::set_dir(Dir::Enter);
                T::delay(T::CHECK_TICK);
                self.run_total(p0 as u32);
            } else {
                T::set_dir(Dir::Exit);
                T::delay(T::CHECK_TICK);
                self.run_total(p3 as u32);
            }
        }
        self.position = position;
        Ok(())
    }

    pub fn run_position_with_async(&mut self, position: i32) -> Result<(), T::Error> {
        T::wait_run_finish();

        if !self.init_flag {
            self.reset().map_err(|e| {
                T::notify_run_finish();
                e
            })?;
        }

        if position > self.position {
            T::set_dir(Dir::Exit);
            T::delay(T::CHECK_TICK);
            self.run_total((position - self.position) as u32);
        } else {
            T::set_dir(Dir::Enter);
            T::delay(T::CHECK_TICK);
            self.run_total((self.position - position) as u32);
        } 
        self.position = position;
        Ok(())
    }

    pub fn run(&mut self, dir: Dir, total: u32) -> Result<(), T::Error> {

        if T::is_alm() {
            return Err(T::ALM_ERR);
        }

        T::wait_run_finish();
        self.init_flag = false;
        T::set_dir(dir);
        self.run_total(total);
        T::wait_run_finish();
        T::notify_run_finish();

        if T::is_alm() {
            return Err(T::ALM_ERR);
        }
        Ok(())
    }

    fn run_total(&mut self, total: u32) {

        if total == 0 {
            T::notify_run_finish();
            return;
        }

        let s0 = if total < (2 * T::VAR_RANGE) {
            total / 2
        } else {
            T::VAR_RANGE
        };
        self.s0.volatile_write(s0);
        self.s1.volatile_write(total - s0);
        self.total.volatile_write(total);
        self.current.volatile_write(0);
        T::enable_pwm(T::BASE_FREQ);
    }

    pub fn run_with_cond<F: Fn() -> bool>(&mut self, cond: F) {
        self.init_flag = false;

        T::set_dir(Dir::Enter);
        T::enable_pwm_not_irq(T::RESET_FREQ);
        loop {
            if T::is_alm() || cond() {
                break;
            }
            T::delay(T::CHECK_TICK);
        }
        T::disable_pwm();
    }

    fn reset(&mut self) -> Result<(), T::Error> {
     
        if !T::is_origin() {
            let mut time_count = 0;
            T::set_dir(Dir::Enter);
            T::enable_pwm_not_irq(T::RESET_FREQ);
            loop {
                if T::is_origin() {
                    break;
                }

                if T::is_alm() {
                    T::disable_pwm();
                    return Err(T::ALM_ERR);
                }

                T::delay(T::CHECK_TICK);
                time_count += T::CHECK_TICK;

                if time_count >= T::RESET_TIMEOUT {
                    T::disable_pwm();
                    return Err(T::TIMEOUT_ERR);
                }
            }
        }
        T::disable_pwm();
        self.reset_slow()?;
        Ok(())
    }

    fn reset_slow(&mut self) -> Result<(), T::Error> {
        T::delay(T::SLOW_DELAY);
        Self::exit2()?;
        Self::exit_origin().map_err(|e| {
            self.init_flag = false;
            e
        })?;
        T::delay(T::SLOW_DELAY);
        Self::enter_origin().map_err(|e| {
            self.init_flag = false;
            e
        })?;
        self.init_flag = true;
        self.position = 0;
        Ok(())
    }

    fn exit2() -> Result<(), T::Error> {
        if T::is_origin() {
            return Ok(());
        }
        let mut count = 0;
        T::set_dir(Dir::Exit);
        T::disable_pwm();
        loop {
            Self::puls_once();
            if T::is_alm() {
                return Err(T::ALM_ERR);
            }
            count += 1;
            if count >= T::EXIT_PULS_COUNT {
                break;
            }
        }
        Ok(())
    }

    fn exit_origin() -> Result<(), T::Error> {

        if !T::is_origin() {
            return Ok(());
        }
        let mut puls_count = 0;

        T::set_dir(Dir::Exit);
        T::disable_pwm();

        loop {
    
            Self::puls_once();
            if !T::is_origin() {
                break;
            } 

            if T::is_alm() {
                return Err(T::ALM_ERR);
            }

            puls_count += 1;
            if puls_count >= T::RESET_PULS_COUNT {
                return Err(T::TIMEOUT_ERR);
            }
        }

        Ok(())
    }

    fn enter_origin() -> Result<(), T::Error> {
        
        if T::is_origin() {
            return Ok(());
        }
        let mut puls_count = 0;
        T::set_dir(Dir::Enter);
        T::disable_pwm();

        loop {
            Self::puls_once();
            if T::is_origin() {
                break;
            }

            if T::is_alm() {
                return Err(T::ALM_ERR);
            }

            puls_count += 1;
            if puls_count >= T::RESET_PULS_COUNT {
                return Err(T::TIMEOUT_ERR);
            }
        }

        Ok(())
    }

    fn puls_once() {
        T::set_puls_pin();
        T::delay(T::SLOW_DELAY_TICK);
        T::reset_puls_pin();
        T::delay(T::SLOW_DELAY_TICK);
    }
}
